/*
 * gyroscope.cpp
 *
 *  Created on: Dec 6, 2012
 *      Author: michiel
 */

#include "navigation/gyroscope.h"

void Gyroscope::joyCallback(const sensor_msgs::Joy::ConstPtr &msg) {
	left_			= msg->axes[PS3_AXIS_ACCELEROMETER_LEFT];
	up_				= msg->axes[PS3_AXIS_ACCELEROMETER_UP];
	yaw_			= msg->axes[PS3_AXIS_GYRO_YAW];
	forward_	= msg->axes[PS3_AXIS_ACCELEROMETER_FORWARD];
}

void Gyroscope::init() {
	joy_sub_ = nh_.subscribe(Topic::PS3_JOY, 10, &Gyroscope::joyCallback, this);
}

void Gyroscope::spin() {
	ros::Rate loop_rate(5);
	while (ros::ok()) {
		ros::spinOnce();
		ROS_INFO("------DATA------");
		ROS_INFO("YAW: %f", yaw_);
		ROS_INFO("LEFT: %f", left_);
		ROS_INFO("UP: %f", up_);
		ROS_INFO("FORWARD: %f", forward_);
		loop_rate.sleep();
	}
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "gyroscope");

	Gyroscope gyro;
	gyro.spin();
}


